/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.networktables2.type.NumberArray;
import java.util.ArrayList;
import java.util.List;


/**
 *
 * @author Programming
 */
public class TargetOverlord {
     Target[] targets = new Target[4];
     boolean hasMiddleTarget;

    public TargetOverlord()
    {
        targets[Target.Type.MIDDLE.getValue()] = new Target(Target.Type.MIDDLE);
        targets[Target.Type.LEFT.getValue()] = new Target(Target.Type.LEFT);
        targets[Target.Type.RIGHT.getValue()] = new Target(Target.Type.RIGHT);
        targets[Target.Type.LOR.getValue()] = new Target(Target.Type.LOR);
    }

    public Position update(NumberArray _bfrCoords,NetworkTable table) {
        getTargetsFromBlobs(pairBlobs(getBlobsVars(getBlobs(_bfrCoords))),table);
        return this.getRobotPosition();
    }

    public List<Blob> getBlobsVars(List<Blob> _blobs) {
        for (int i = 0; i < _blobs.size(); i++) {
            _blobs.get(i).getAspectRatio();
            _blobs.get(i).getCenter();
            _blobs.get(i).getArea();
        }

        return _blobs;
    }

    public List<daBlob> pairBlobs(List<Blob> blobs){
        ArrayList<daBlob> returnBlobs = new ArrayList<daBlob>();

        for (int i = 0; i < blobs.size(); i++) {
            if (blobs.get(i).area < 100) {
                blobs.remove(i);
            }
        }

        for (int x = 0, y = 0; x < blobs.size(); x++){
            for (y = x + 1; y < blobs.size(); y++){
                if (CompareBlobs(blobs.get(x),blobs.get(y))){
                    returnBlobs.add(new daBlob(blobs.get(x),blobs.get(y)));
                    Blob t = blobs.get(x);
                    Blob t2 = blobs.get(y);
                    blobs.remove(t);
                    blobs.remove(t2);
                    x=y=0;
                }
            }
        }
        if (!blobs.isEmpty()){
            System.out.println(blobs.size() + " blobs didnt find a friend!!!");
        }
        return returnBlobs;
    }

    public boolean CompareBlobs(Blob b1, Blob b2){
        int points = 0;
        if ((b1.center[0] <= b2.center[0] + 9.0) || (b1.center[0] >= b2.center[0] - 9.0)) {
            points ++;
        }
        if (points > 1) {
            System.out.println(" Blobs made friends!!!");
        }
        return (points > 0);
    }
 
    public List<Blob> getBlobs(NumberArray bfrCoords){
        List<Blob> returnBlobs;
        List<Blob> rawBlobs = new ArrayList<Blob>();
        for(int i = 0; i < bfrCoords.size(); i+=8) {
            rawBlobs.add(i/8, new Blob(bfrCoords.get(i), bfrCoords.get(i + 1), bfrCoords.get(i + 2), bfrCoords.get(i + 3),
                                       bfrCoords.get(i + 4), bfrCoords.get(i + 5), bfrCoords.get(i + 6), bfrCoords.get(i + 7)));
        }

        return rawBlobs;
    }

    public void getTargetsFromBlobs(List<daBlob> rawBlobs,NetworkTable table) {
       //find center
        targets[Target.Type.LOR.getValue()].clear();
        targets[Target.Type.LEFT.getValue()].clear();
        targets[Target.Type.RIGHT.getValue()].clear();
        targets[Target.Type.MIDDLE.getValue()].clear();

        double  middleCenterX = 0;

        //find the middle target

        for (int i = 0; i < rawBlobs.size(); i++) {
            double ratio = rawBlobs.get(i).blobInner.getAspectRatio();
           // System.out.println("Ratio:" + ratio);
            if (ratio < (Target.targetMiddleAspectRatio + Target.ratioThreshold) && ratio > (Target.targetMiddleAspectRatio - Target.ratioThreshold)) {
                targets[Target.Type.MIDDLE.getValue()].setBlob(rawBlobs.get(i), table); //set middles blobs
                targets[Target.Type.MIDDLE.getValue()].initalized = true;
                middleCenterX = targets[Target.Type.MIDDLE.getValue()].centerInnerX;
                rawBlobs.remove(i);//if something goes wrong its probably this
                System.out.print("Middle found!");
                break;
            }
        }

        for (int i = 0; i < rawBlobs.size(); i++) {
            double ratio = rawBlobs.get(i).blobInner.getAspectRatio();
            double [] center = rawBlobs.get(i).blobInner.getCenter();

            if((ratio < (Target.targetLORAspectRatio + Target.ratioThreshold) && ratio > Target.targetLORAspectRatio - Target.ratioThreshold)) //is left or right
            {
                if (targets[Target.Type.MIDDLE.getValue()].initalized){  //if we saw the middle
                    if (center[0] < middleCenterX) {      //if it is left of the middle
                        if (targets[Target.Type.LEFT.getValue()].initalized)
                            System.out.println("UMM BRO the left target was already initilized(TargetOverload.Java)");
                        targets[Target.Type.LEFT.getValue()].setBlob(rawBlobs.get(i),table);
                        targets[Target.Type.LEFT.getValue()].initalized = true;
                        System.out.println("Found Left");
                    }
                    else { //if were not left of the center were right of it
                        if (targets[Target.Type.LEFT.getValue()].initalized)
                            System.out.println("UMM BRO the right target was already initilized(TargetOverload.Java)");
                        targets[Target.Type.RIGHT.getValue()].setBlob(rawBlobs.get(i),table);
                        targets[Target.Type.RIGHT.getValue()].initalized = true;
                        System.out.println("Found Right");
                    }
                }
             else { // if we dont see the middle we dont know what target it is so set LOR to it
                    if (targets[Target.Type.LEFT.getValue()].initalized)
                        System.out.println("UMM BRO the right target was already initilized(TargetOverload.Java)");
                    targets[Target.Type.LOR.getValue()].setBlob(rawBlobs.get(i),table);
                    targets[Target.Type.LOR.getValue()].initalized = true;
                    System.out.println("Found LOR");
                   }
            }
        }
        hasMiddleTarget = targets[Target.Type.MIDDLE.getValue()].initalized;
        System.out.println();
    }
    public Target.Type getBestTarget(){
        return Target.Type.MIDDLE;
    }

    public Position getRobotPosition() {
	List<Position> positions = new ArrayList<Position>();

        if (!targets[Target.Type.LOR.getValue()].initalized)
        for (int a = 0; a < 3; a++) // colect results
	{
		if (!targets[a].initalized)
                    continue;

		positions.add(getRobotPosition(targets[a].type));

		for (int b = 0; b < 3; b++)
		{
			if (!targets[b].initalized)
				continue;
			if (Target.getTargetX(targets[a].type) == Target.getTargetX(targets[b].type))
				continue;
			//positions.add(getRobotPosition(targets[a].type,targets[b].type));
		}
	}
        if(targets[Target.Type.LOR.getValue()].initalized){
            positions.add(getRobotPosition(Target.Type.LOR));
        }


	//confidence = (int)positions.size() * 5;

	Position firstAverage = new Position(0,0);
	if (positions.isEmpty()) {
            System.out.println("No positions founnd");
            return firstAverage; // set to zero
	}

	System.out.println("postions:" + positions.size());
	List<Position> validatedPositions = new ArrayList<Position>();

	for (int a = 0; a < positions.size(); a ++ ) { //initial average
		firstAverage.x += positions.get(a).x;
		firstAverage.z += positions.get(a).z;
	}

	firstAverage.x /= positions.size();
	firstAverage.z /= positions.size(); // set return pos to the average

	for (int a = 0; a < positions.size(); a ++ ){
		positions.get(a).SetError(firstAverage);
                //positions.get(a).Print();
	} // set error

	boolean change = true;
	while(change) // order the targets
	{
		change = false;
		for (int a = 0; a < positions.size() - 1; a ++ ) {
			if (positions.get(a).error > positions.get(a + 1).error) {
				change = true;
				Position temp = positions.get(a);
				positions.set(a, positions.get(a + 1));
				positions.set(a + 1, temp) ;
			}
		}
	}

	
	int newSize = (int)Math.ceil((positions.size() + 1.0)/2);
        List<Position> tempPositions = new ArrayList<Position>();
        for (int x = 0; x< newSize;x+=1){
            tempPositions.add(positions.get(x));
        }

	Position returnPos = new Position(0,0);

	for ( int a = 0; a < newSize; a ++ )
	{
		returnPos.x += tempPositions.get(a).x;
		returnPos.z += tempPositions.get(a).z;
	}


	returnPos.x /= newSize;
	returnPos.z /= newSize; // set return pos to the average



	return returnPos;//positions.front();

    }

 public Position getRobotPosition(Target.Type _target) {
        Position robotPosition = new Position();
	Target target = targets[_target.getValue()];

	robotPosition.x = Math.sin(target.azimuth)*target.distance;
	robotPosition.z = Math.cos(target.azimuth)*target.distance;
	robotPosition.x += Target.getTargetX(_target);
	double targetAzimuth = Math.atan(robotPosition.x/robotPosition.z) + Math.PI;
	double camAzimuth = targetAzimuth - target.cameraAngle;
	double camCos = Math.cos(camAzimuth);
	double camSin = Math.sin(camAzimuth);

	robotPosition.x -= Target.cameraHeight * camSin + Target.cameraHorizontalOffset * camCos;
	robotPosition.z -= Target.cameraHeight * camCos - Target.cameraHorizontalOffset * camSin; //if anything goes wrong it's probably this

	return robotPosition;
    }

    public Position getRobotPosition(Target.Type _target1, Target.Type _target2) {
	Target target1 = targets[_target1.getValue()];
        Target target2 = targets[_target2.getValue()];
        Position position = new Position(0,0);
	if (Target.getTargetX(_target1) != Target.getTargetX(_target2)) {
		double dist1 = target1.distance;
		double dist2 = target2.distance;
		double angleDiff = target2.cameraAngle - target1.cameraAngle;
		double xDiff = Target.getTargetX(_target2) - Target.getTargetX(_target1);
		//improve target1's azimuth
		target1.azimuth = Math.acos(dist2*Math.sin(angleDiff) / xDiff);
		if(Double.isInfinite(target1.azimuth) || Double.isNaN(target1.azimuth))
			target1.azimuth = 0; // check for infinites,negative square roots and other gargbage
		
		double x = Math.sin(target1.azimuth) * dist1;
		double z = Math.cos(target1.azimuth) * dist1;
		position.x = x;
		position.z = z;
		//position.x += GetTargetX(_targPos2);
	}
	else {
		System.out.println("Failure in TargetOverlord.getRobotPosition (2 args), uninitialized position line 372");
	}
	return position;

    }
}
